package robocup.component.worldmodel;

import java.util.Arrays;
import java.util.Vector;

import robocup.component.DAInamiteConstants;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.ControllerNumbers.REFEREE_MESSAGE;
import robocup.component.actions.Action;
import robocup.component.actions.CatchAction;
import robocup.component.actions.DashAction;
import robocup.component.actions.MoveAction;
import robocup.component.actions.TurnAction;
import robocup.component.filters.MCLFilter;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.FlagInfo;
import robocup.component.infotypes.InfoReceiver;
import robocup.component.infotypes.LineInfo;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.worldmodel.particlefilter.ParticleFilter;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Flag;
import robocup.component.worldobjects.Line;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.StaticObject;
import robocup.component.worldobjects.Player.RELATIVE;

/**
 * MeModel is the part of the WorldModel which implements functions to calculate
 * and store the actual state of the player itself (position, speed, direction
 * etc.). It is updated each time an info arrives.
 */
public class MeModel
    implements InfoReceiver {

  // the world for other worldmodel parts methods calls
  private final WorldModel     world;

  private final SConf          sConf          = SConf.getInstance();

  // the player
  private Player         me             = Player.getDummyInstance();

  // the me of last cycle
  private final Player         lastMe         = Player.getDummyInstance();

  // the me of as predicted for this cycle
  // based on the last world and the last action
  private final Player         predictedMe    = Player.getDummyInstance();

  // the me after bs
  private final Player         tmpMe          = Player.getDummyInstance();

  // the view width of the player in degree (91 for debugging ;-)
  private double         viewWidth      = 91;

  // particle filter for position estimation (high quality)
  private final ParticleFilter particleFilter = new ParticleFilter();

  // particle filter for position estimation (fast but low quality)
  private final MCLFilter      mclFilter;

  private int            playerType;

  //buffer for all player positions calculated from a combination of 2 flags
  private final Vektor[] estimatedPlayerPositions = Vektor.getDummyInstances(
      DAInamiteConstants.WM_MAX_FLAGS * DAInamiteConstants.WM_MAX_FLAGS / 2);
  
  //buffer for all head dirs calculated from a combination of 2 flags
  private final double[] flagsHeadDirs = new double[DAInamiteConstants.WM_MAX_FLAGS 
                                              * DAInamiteConstants.WM_MAX_FLAGS 
                                              / 2];
  
  //dummy for the vector from the self to a flag
  private final Vektor toFlagDummy = Vektor.getDummyInstance();
  private final Vektor dummyVektor = Vektor.getDummyInstance();
  private final Vektor dummyVektor2 = Vektor.getDummyInstance();

  private boolean outside= false;;

  /**
   * <code>MeModel</code> constructor - a model containing all information
   * about the self-agent.
   * 
   * @param world -
   *          the current world-model (parent)
   */
  public MeModel(
      final WorldModel world) {

    this.mclFilter = new MCLFilter();
    this.world = world;
  }

  /**
   * This method overwrites the me-player, created after receiving the
   * init-command. This way all models are working on the same reference.
   * 
   * @param self -
   *          the new me-reference
   */
  public void initSelf(final Player self) {

    this.me = self;
  }

  /**
   * updateMeSense updates the player <code>me</code> from the SenseBodyInfo
   * <code>sbInfo</code>. Some knowledge is explicitly given by the
   * <code>sbInfo</code>. Other knowledge has to be estimated based on the
   * last wolrd state and the performed actions.
   * 
   * @param sbInfo -
   *          Info for the update
   */
  public void update(final SenseBodyInfo sbInfo) {

    // save last me
    this.lastMe.copy(this.me);

    // set the expected me (used for collision model)
    if (sbInfo.getLastSentActions() != null) {
      this.predictedMe.copy(this.world.getSelfRef());
      for (final Action a : sbInfo.getLastSentActions()) {
        this.predictedMe.predictThisAfterAction(a);
      }
    }

    // The following code calculates a prediction of the player's speed,
    // position, body and head angle based on the information of the
    // last cycle and the information contained in the sense body info.
    // View UvA master's thesis chapter 6.4.1 for further information.
    // Complain to till, if it does not work!
    double lastActionTurnAngle = 0.0;
    Vector<Action> lastSentActions;

    this.viewWidth = sbInfo.getViewWidth().getFactor() * 
    	this.sConf.VISIBLE_ANGLE;
    
    // set new body direction to old body direction
    double newBodyDir = this.me.getBodyDir();

    double stamina = this.me.getStamina();
    double recovery = this.me.getRecovery();

    // if actions were sent in the last cycle
    if ((lastSentActions = sbInfo.getLastSentActions()) != null) {
      // estimate the new player based on the last world and the
      // sent actions (see UvA Master thesis for details)
      for (final Action lastAction : lastSentActions) {
        switch (lastAction.getType()) {
          case TURN:
            final TurnAction tAction = (TurnAction) lastAction; // changed

            lastActionTurnAngle += tAction.getAngle();
            if (lastActionTurnAngle > this.sConf.MAXMOMENT) {
              lastActionTurnAngle = this.sConf.MAXMOMENT;
            }
            else if (lastActionTurnAngle < -this.sConf.MAXMOMENT) {
              lastActionTurnAngle = -this.sConf.MAXMOMENT;
            }
            // Note that equation 6.9 of the master's thesis is
            // incorrect.
            // The effective turn angle is calculated as follows:
            final double speed = this.me.getSpeed().getLength();
            // equation 3.31 of master's thesis says: eff =
            // last/(1+intert*speed)
            // equation 6.9 of master's thesis says: eff =
            // last/(1+inert)*speed
            // equation 6.9 seems to be incorrect!
            double effectiveTurnAngle = 0.0;
            effectiveTurnAngle = lastActionTurnAngle / (1.0 + this.me
                .getPConf().INERTIA_MOMENT * speed);
            newBodyDir += effectiveTurnAngle;
            newBodyDir = Vektor.normalize(newBodyDir);
            break;

          case CATCH_BALL:
            final CatchAction cAction = (CatchAction) lastAction;
            newBodyDir += cAction.getDirection();
            newBodyDir = Vektor.normalize(newBodyDir);
            break;

          case MOVE:
            if (!SConf.getInstance().COACH) {
              // after a MOVE action, the actual pos and speed in WM
              // are not valid anymore
              final MoveAction mAction = (MoveAction) lastAction;
              final double posX = mAction.getX();
              final double posY = -mAction.getY();
              final Vektor pos = new Vektor(Vektor.XY, posX, posY);
              pos.rotate(-90);
              // reset the filter
              this.mclFilter.reset(pos);
              this.me.setCycle(sbInfo.getCycle());
              this.me.setPosition(pos);
              this.me.getSpeed().reset();
            }
            break;

          case DASH:
            double power = ((DashAction) lastAction).getPower();
            if (power > this.sConf.MAXPOWER) {
              power = this.sConf.MAXPOWER;
            }
            else if (power < this.sConf.MINPOWER) {
              power = this.sConf.MINPOWER;
            }

            // calculate stamina for calculating recovery according to
            // MT Alg. 3.1: stamina model algorithm

            // reduce stamina according to dash power
            if (power < 0) {
              stamina -= 2 * Math.abs(power);
            }
            else {
              stamina -= power;
            }
            break;

          case TACKLE:
            // tackle counter arrives directly in bs-info
            break;

          case KICK:
            if (PLAY_MODE.META_STANDARD_OWN.contains(this.world
                .getPreviousPlaymode()) && this.world
                .getLastPlayModeMessageCycle() > this.world.getBodyCycle() - 2) {
              // remember that this player did the standard and therefor
              // is not allow to intercept the ball!!
              this.me.setKickedStandard(true);
            }
            else {
              this.me.setKickedStandard(false);
            }

          default:
            ;

        }
      } // end for
    } // end if

    // set new position to old position
    final Vektor newPos = this.me.getPosition().cloned();

    final double speedDir = Vektor
        .normalize(newBodyDir + sbInfo.getNeckAngle() + sbInfo.getSpeedDir());

    final Vektor newSpeed = new Vektor(sbInfo.getSpeed(), speedDir);

    if (newSpeed.getLength() > this.me.getPConf().PLAYER_SPEED_MAX * this.me
        .getPConf().PLAYER_DECAY) {
      newSpeed.setLength(this.me.getPConf().PLAYER_SPEED_MAX * this.me
          .getPConf().PLAYER_DECAY);
    }

    final Vektor oldSpeed = newSpeed.cloned();
    oldSpeed.multThis(1.0 / this.me.getPConf().PLAYER_DECAY);

    // new position depends on the old speed
    newPos.addToThis(oldSpeed);

    // send odometry data to the filter
    this.mclFilter.setU(oldSpeed);

    // BEGIN calculate recovery according to MT Alg. 3.1: stamina model
    // algorithm reduce recovery when stamina is below recovery decrement
    // threshold
    if (stamina <= this.sConf.RECOVER_DEC_THR * this.sConf.STAMINA_MAX) {
      if (recovery > this.sConf.RECOVER_MIN) {
        recovery -= this.sConf.RECOVER_DEC;
      }
      recovery = Math.max(this.sConf.RECOVER_MIN, recovery);
    }
    // algorithm

    this.particleFilter.translatePosition(oldSpeed);

    this.me.setCycle(sbInfo.getCycle());
    this.me.setPosition(newPos);
    this.me.setSpeed(newSpeed);
    this.me.setBodyDir(newBodyDir);
    this.me.setNeckAngle(sbInfo.getNeckAngle());
    this.me.setStamina(sbInfo.getStamina());
    this.me.setEffort(sbInfo.getEffort());
    this.me.setRecovery(recovery);
    // point at status, absolute angle
    this.me.setArmDirection(Vektor
        .normalize(sbInfo.getArmDir() + (newBodyDir + sbInfo.getNeckAngle())));
    this.me.setArmDistance(sbInfo.getArmDist());
    this.me.setArmExpires(sbInfo.getArmExpires());
    this.me.setArmMovable(sbInfo.getArmMoveable() == 0);
    this.me.setTackledeadCountdown(sbInfo.getTackleExpires());

    this.tmpMe.copy(this.me);
  }

  /**
   * <code>updateVisual</code> updates the player <code>me</code> from the
   * VisualInfo <code>vInfo</code>. Of special importance here is the update
   * of the players head direction and its position as these data has a huge
   * impact on most of the world knowledge. Because of that the MeModel is the
   * 1st model to be updated after an VI arrival.
   * 
   * There exist many sophisticated methods for calculating a player's position.
   * This method uses 2 algorithms. The first simply takes some near flags. The
   * 2nd is more complicated and uses a particle filter similar to the one
   * described in the UvA master's thesis (see manual). As the position quality
   * is of less importance with no ball near we use the 1st and faster method
   * for distant balls and the 2nd for near ones.
   * 
   * @param vInfo -
   *          new visual info
   */
  public void update(final VisualInfo vInfo) {

    // tests shown, that only one line can be seen pro cycle
    final LineInfo[] lines = vInfo.getLines();
    LineInfo line = null;
    if (lines.length != 0) {
      line = lines[0];
    }
    if (lines.length != 1) {
      this.outside= true;
    }
    else {
      this.outside= false;
    }

    final FlagInfo[] flags = vInfo.getFlags();
    Arrays.sort(flags);    

    final int maxIndex = Math.min(flags.length, DAInamiteConstants.WM_MAX_FLAGS);
    
    if(maxIndex < 2)
      return;
    
    int count = 0;
    
    /*
     * calculate our estimated position using a slightly modified version of
     * the MT algorithm
     */
    for (int i = 0; i < maxIndex - 1 && count < DAInamiteConstants.WM_MAX_FLAGS; i++) {
      for (int j = i + 1; j < maxIndex && count < DAInamiteConstants.WM_MAX_FLAGS; j++) {
        this.approxMyPos(flags[i], flags[j], count);
        count++;
      }
    }
    
    if(count < estimatedPlayerPositions.length){
      estimatedPlayerPositions[count].setEmpty();
    }

    if(count < flagsHeadDirs.length){
      flagsHeadDirs[count] = Double.NaN;
    }

    // Vektor temp = averagePosArth(flagsPositions);
    Vektor temp = this.averagePosStdDev(estimatedPlayerPositions);
    // Vektor temp = averagePosRMS(flagsPositions);
    Vektor newPos = this.me.getPosition().cloned();

    double headAngle;
    // now let's calculate the position using flags & line
    if (line != null && temp != null) {
      /*
       * We know approximately where we are (newPos). This enables us an exact
       * calculation of our headangle from the seen line.
       */
      if (!outside) {
        headAngle = this.getHAFromLine(line, newPos);
      }
      else {
        headAngle= getHeadDir();
        if (PLAY_MODE.META_STANDARD_OTHER.contains(world.getPlayMode())) {
          //maybe set out
          if (world.getSelfRef().getDistance(world.getBallRef())<9.15) {
            Vektor moveVektor= world.getSelfRef().getPosition().sub(world.getBallRef().getPosition());
            moveVektor.setLength(9.15);
            moveVektor.addToThis(world.getBallRef().getPosition());
            world.getSelfRef().setPosition(moveVektor);
          }
          else {
//            System.out.println(world.getCycleAndPlayerString()+" outside before");
          }
        }
        return;
      }
      /*
       * Now we know our exact headangle. Thanks to this information we can
       * calculate the Vektors from our position to the flags, because we have
       * the distance to each flag and the angle, realtive to our own
       * headangle. Than we substract these Vektors from the positions of the
       * flags. This way we get one approximated position of the player for
       * each flag we see. The last step is to calculate the average position
       * from a Vector of all approximated positions We use only WM_MAX_FLAGS
       * for the position calculation
       */

      if (this.world.getDistance(this.world.getBallRef()) < 15) {
        // this.particleFilter.resetPosition(null);
        for (int i = 0; i < maxIndex; i++) {
          // Vektor from the player to the flag
          this.toFlagDummy.setVektor(flags[i].getDistance(),
              headAngle + flags[i].getDirection());
          
          // We need to handle the "felt" flags that have no IDs
          // (-1)
          Vektor flagpos = null;

          if (flags[i].getId() == -1) {
            /*
             * There is a chance that we can find out, which flag we "feel".
             * The method SConf.getFlag(Vektor position) returns a flag if the
             * position lies near it. Near means in a circle around the flag
             * (radius of the circle is DAInamiteConstants.WM_FLAG_RADIUS)
             */
            final Flag f = this.sConf.getFlag(this.toFlagDummy.addToThis(newPos));
            if (f != null) {
              // a flag has been found
              flagpos = f.getPosition();
            }
          }
          else {
            // we can get the flag directly by its ID
            flagpos = this.sConf.getFlag(flags[i].getId()).getPosition();
          }
          this.particleFilter.addFlag(flags[i], headAngle, flagpos);

        }
        temp = this.particleFilter.getPosition();
        if (temp != null && !Double.isNaN(temp.x) && !Double.isNaN(temp.y)) {
          newPos = temp;
          double anglesin = 0;
          double anglecos = 0;
          long flagcount = 0;
          
          for (int i = 0; i < maxIndex; i++) {
            // Vektor from the player to the flag
            this.toFlagDummy.setVektor(flags[i].getDistance(),
                headAngle + flags[i].getDirection());
            // We need to handle the "felt" flags that have no
            // IDs (-1)
            Vektor flagpos = null;

            if (flags[i].getId() == -1) {
              /*
               * There is a chance that we can find out, which flag we "feel".
               * The method SConf.getFlag(Vektor position) returns a flag if
               * the position lies near it. Near means in a circle around the
               * flag (radius of the circle is
               * DAInamiteConstants.WM_FLAG_RADIUS)
               */
              final Flag f = this.sConf.getFlag(this.toFlagDummy.addToThis(newPos));

              if (f != null) {
                // a flag has been found
                flagpos = f.getPosition();
              }
            }
            else {
              // we can get the flag directly by its ID
              flagpos = this.sConf.getFlag(flags[i].getId()).getPosition();
            }

            final double x = -temp.x + flagpos.x + Double.MIN_VALUE;
            final double y = -temp.y + flagpos.y + Double.MIN_VALUE;
            final double angle = (Math.atan2(x, y)) - Math.PI * flags[i]
                .getDirection() / 180.0;
            anglesin += Math.sin(angle) + Double.MIN_VALUE;
            anglecos += Math.cos(angle) + Double.MIN_VALUE;
            flagcount++;
          }

          // anglesin /= (double)flagcount;
          // anglecos /= (double)flagcount;

          headAngle = 180.0 * Math.atan2(anglesin, anglecos) / Math.PI - 0.4;
        }
      }
      else {
        count = 0;
        for (int i = 0; i < maxIndex && count < DAInamiteConstants.WM_MAX_FLAGS; i++) {
          // Vektor from the player to the flag
          this.toFlagDummy.setVektor(flags[i].getDistance(),
              headAngle + flags[i].getDirection());
          // We need to handle the "felt" flags that have no IDs
          // (-1)
          if (flags[i].getId() == -1) {
            /*
             * There is a chance that we can find out, which flag we "feel".
             * The method SConf.getFlag(Vektor position) returns a flag if the
             * position lies near it. Near means in a circle around the flag
             * (radius of the circle is DAInamiteConstants.WM_FLAG_RADIUS)
             */
            this.dummyVektor.copy(toFlagDummy).addToThis(newPos);
            final Flag f = this.sConf.getFlag(this.dummyVektor);
            if (f != null) {
              // a flag has been found
              this.dummyVektor.copy(f.getPosition()).subFromThis(this.toFlagDummy);
              this.mclFilter.sampleParticle(this.dummyVektor,
                  flags[i].getDistance());
              count++;
            }
          }
          else {
            // we can get the flag directly by its ID
            final Vektor flag = this.sConf.getFlag(flags[i].getId()).getPosition();
            this.dummyVektor.copy(flag).subFromThis(this.toFlagDummy);
            this.mclFilter.sampleParticle(this.dummyVektor, flags[i]
                .getDistance());
            count++;
          }
        }

        // average position
        // newPos = averagePosStdDev(flagsPositions);
        temp = this.mclFilter.getPosition();
        if (temp != null && !Double.isNaN(temp.x) && !Double.isNaN(temp.y)) {
          newPos = temp;
          // System.out.println("[MeModel] newPos: "+newPos);
        }
        // newPos = averagePosArth(flagsPositions);
      }
    }
    else {
      /*
       * The player didn't see any line, so we use the unprecise calculation
       * from MT for the position. We also have to approximate the average
       * headangle.
       */
      // headAngle = averageHAArth(flagsHeadDirs);
      // rw for debugging System.out.println("No Line Seen");
      headAngle = this.averageHAStdDev(flagsHeadDirs);
    }
    // safety first ;)
    if (Double.isNaN(headAngle)) {
      headAngle = 0;
      this.me.setBodyDir(0.0);
      this.me.setNeckAngle(0.0);
      this.me.getPosition().reset();
      this.me.getSpeed().reset();
      this.me.getLastSeenPosition().reset();
      this.lastMe.setBodyDir(0.0);
      this.lastMe.setNeckAngle(0.0);
      this.lastMe.getPosition().reset();
      this.lastMe.getSpeed().reset();
      this.lastMe.getLastSeenPosition().reset();
      this.mclFilter.reset(this.me.getPosition());
    }
    else {
      this.me
          .setBodyDir(Vektor.normalize(headAngle - this.me.getNeckAngle()));
    }

    this.me.setPosition(newPos);
    if (this.world.collided()) {
      this.mclFilter.reset(newPos);
    }

    this.me.setCycle(vInfo.getCycle());
    this.me.setBodyCycle(this.world.getBodyCycle());
  }

  /**
   * <code>updateAural</code> updates the player <code>me</code> from the
   * AuralInfo <code>aInfo</code>
   * 
   * @param aInfo
   */
  public void update(final RefereeInfo info) {

    // reset recovery after half time
    if ((info).getRefereeMessageType() == REFEREE_MESSAGE.HALF_TIME) {
      this.me.setRecovery(this.sConf.RECOVER_MAX);
    }
  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayModeInfo)
   */
  public void update(final PlayModeInfo info) {

  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayerSayInfo)
   */
  public void update(final PlayerSayInfo info) {

    if (info == null) {
      return;
    }
    this.me.setLastHeared(this.world.getBodyCycle(), info.getNumber(),
        this.world.getMessageFactory().getMessageTypes().get(0));
  }

  /**
   * the difference between the current and (the last position + last speed)
   * 
   * @return
   */
  public Vektor getSelfPositionEstimationError() {

    return this.me.getPosition().sub(this.tmpMe.getPosition());
  }

  /**
   * <code>approxMyPos</code> calculates player's position from 2 FlagInfo
   * objects.
   * 
   * @param f1 -
   *          FlagInfo 1
   * @param f2 -
   *          FlagInfo 2
   * @param idx -
   *          the index of the estimatedPlayerPositions and flagsHeadDirs arrays
   *          to write the result to (not pretty but saves resources)
   * @return Vektor - player's position
   */
  private final void approxMyPos(final FlagInfo f1,
      final FlagInfo f2, final int idx) {

    try {
      if (f1.getId() == -1 || f2.getId() == -1) {
        return;
      }
      final Vektor p1 = this.sConf.getFlag(f1.getId()).getPosition();
      final Vektor p2 = this.sConf.getFlag(f2.getId()).getPosition();
      final double pp1 = f1.getDistance();
      final double pp2 = f2.getDistance();

      this.dummyVektor2.copy(this.getNewPosition(p1, pp1, Math.toRadians(f1
          .getDirection()), p2, pp2, Math.toRadians(f2.getDirection()),
          this.dummyVektor, this.dummyVektor2));

      this.dummyVektor.copy(p2).subFromThis(this.dummyVektor2);
      this.dummyVektor.rotate(-f2.getDirection());
      double newHeadAngle = this.dummyVektor.getAngle();
      this.dummyVektor.copy(p1).subFromThis(this.dummyVektor2);
      this.dummyVektor.rotate(-f1.getDirection());
      newHeadAngle += this.dummyVektor.getAngle();
      newHeadAngle /= 2;

      if (Double.isNaN(newHeadAngle)) {
        newHeadAngle = this.me.getHeadDir();
      }

      this.estimatedPlayerPositions[idx].copy(this.dummyVektor);
      this.flagsHeadDirs[idx] = newHeadAngle;
    } // end try
    catch (final NullPointerException npe) {
      npe.printStackTrace();
    } // end catch
  }

  /**
   * <code>getHAFromLine</code> returns the HeadAngle of the player looking at
   * the line <code>lineInfo</code> & being at <code>pos</code>
   * 
   * @param lineInfo -
   *          line the player stares at
   * @param pos -
   *          position of the player
   * @return double - headangle of the player
   */
  private double getHAFromLine(final LineInfo lineInfo,
      final Vektor pos) {

    final double lineDir = lineInfo.getDirection();
    // horizontal line
    if (this.isHorizontal(lineInfo)) {
      final double lineY = this.sConf.getLine(lineInfo.getId()).getPoint1().y;
      final boolean overLine = (lineY < pos.y);
      // positive lineAngle
      if (lineDir > 0) {
        return (overLine) ? -90 - lineDir : 90 - lineDir;
      } // end if

      // negative lineAngle
      return (overLine) ? 90 - lineDir : -90 - lineDir;
    } // end if

    // Vertical line
    final double lineX = this.sConf.getLine(lineInfo.getId()).getPoint1().x;
    final boolean rightOfLine = (lineX < pos.x);
    // positive lineAngle
    if (lineDir > 0) {
      return (rightOfLine) ? -lineDir : 180 - lineDir;
    } // end if

    // negative lineAngle
    return (rightOfLine) ? -180 - lineDir : -lineDir;
  }

  /**
   * <code>getNewPosition</code> returns the position of the player. This
   * method implements the position calculation algorithm from the MT (6.4.2.1,
   * Method 2). There is a slight modification however. <code>h</code> is
   * calculated using Pitagoras instead of cosinus, which seems to be better
   * (test with trainer when available!). This method works well, but the
   * algorithm is not too efficient. This is not critical, since we only use it
   * for a preliminary approximation most of the time.
   * 
   * @param f -
   *          position of the first Flag
   * @param fr -
   *          distance from player to the first Flag
   * @param fAngle -
   *          angle between of the first Flag (rel. to HA)
   * @param g -
   *          position of the second Flag
   * @param gr -
   *          distance from player to the second Flag
   * @param gAngle -
   *          angle between of the second Flag (rel. to HA)
   * @return Vektor - position of the player
   */
  private Vektor getNewPosition(final Vektor f,
      final double fr,
      final double fAngle,
      final Vektor g,
      final double gr,
      final double gAngle,
      final Vektor dummyV,
      final Vektor resultV) {

    final double fx = f.x, fy = f.y, gx = g.x, gy = g.y;
    final double deltaX = (gx - fx), deltaY = (gy - fy);
    if (deltaX == 0 && deltaY == 0) {
      return null;
    } // end if
    final double d = Math.sqrt((deltaX) * (deltaX) + (deltaY) * (deltaY));
    final double a = (fr * fr - gr * gr + d * d) / (2 * d);
    // TODO Smarqoo, Feb 14, 2005, 1:44:13 AM, Test cosinus with trainer
    // less Vektor creation (rw)
    final double h = Math.sqrt(Math.abs(fr * fr - a * a));
    // final double h = Math.cos(alpha) * fr;

    dummyV.copy(g).subFromThis(f);
    dummyV.setLength(a);
    boolean fLeft = (fAngle < gAngle) ? true : false;
    fLeft = (a < 0) ? !fLeft : fLeft;
    final double rotate = (fLeft) ? dummyV.getAngle() + 90 : dummyV.getAngle() - 90;
    
    resultV.setVektor(h, rotate);
    resultV.addToThis(f).addToThis(dummyV);
    
    return resultV;
  }

  /**
   * <code>averagePosStdDev</code> calculates the average position from a
   * Vector of Vektors using standard deviation weights
   * 
   * @param positions -
   *          Vector with positions (Vektors)
   * @return Vektor - average position
   */
  private final Vektor averagePosStdDev(final Vektor[] positions) {

    if (positions == null || positions[0].isEmpty()) {
      return null;
    }
    final Vektor av = new Vektor(0, 0);
    int count = 0;
    final Vektor[] posArray = new Vektor[positions.length];

    for (final Vektor v : positions) {
      if (!v.isEmpty()) {
        // rw was macht denn dieses if? koennen x und y NaN sein??
        if (!(Double.isNaN(v.x) || Double.isNaN(v.y))) {
          posArray[count] = v;
          av.addToThis(v);
          count++;
        }
      }
      else
        break;
    }

    if (count == 0) {
      return null;
    }

    final double[] xWeights = new double[count];
    final double[] yWeights = new double[count];
    // means
    final double xMean = av.x / count;
    final double yMean = av.y / count;

    // rw added +1 for avoiding div 0
    for (int i = 0; i < count; i++) {
      // weight (1/sigma^2)
      xWeights[i] = 1 / ((posArray[i].x - xMean) * (posArray[i].x - xMean) + 1);
      yWeights[i] = 1 / ((posArray[i].y - yMean) * (posArray[i].y - yMean) + 1);
    }

    double xWeightSum = xWeights[0];
    double yWeightSum = yWeights[0];
    double weightedXSum = xWeights[0] * posArray[0].x;
    double weightedYSum = yWeights[0] * posArray[0].y;
    for (int i = 1; i < count; i++) {
      weightedXSum += xWeights[i] * posArray[i].x;
      weightedYSum += yWeights[i] * posArray[i].y;
      xWeightSum += xWeights[i];
      yWeightSum += yWeights[i];
    }

    // return our beautiful result ;)
    return new Vektor(Vektor.XY, weightedXSum / xWeightSum,
        weightedYSum / yWeightSum);
  }

  /**
   * <code>averageHAStdDev<code> calculates the average HeadAngle from a 
   * Vector of angles using standard deviation weights.
   * 
   * @param headDirections    - Vector with various head angles (double)
   * @return double           - average head angle
   */
  private double averageHAStdDev(final double[] headDirections) {

    if (headDirections == null || headDirections[0] == Double.NaN) {
      // rw bitte dann auch abfangen!! (und mich nicht nach NaNs suchen
      // lassen)
      // sollten werte nicht berechnet werden kï¿½nnen (was an sich
      // schon komisch wï¿½re)
      // so nehmt bitte die geschï¿½tzten werte aus dem prophet
      return Double.NaN;
    } // end if

    int count = 0;
    double mean = .0;
    for (final double element : headDirections) {
      if (!Double.isNaN(element)) {
        mean += Vektor.normalize(element);
        count++;
      }
      else
        break;
    }

    mean /= count;
    final double[] weights = new double[count];
    for (int j=0; j< weights.length; j++) {
      // weight (1/sigma^2)
      // rw added +1 avoiding div 0
      weights[j] = 1 / ((headDirections[j] - mean) * (headDirections[j] - mean) + 1);
      j++;
    }

    double weightSum = .01;
    double weightedSum = .01;
    int i = 0;
    for (final double weight : weights) {
      weightSum += weight;
      weightedSum += weight * headDirections[i];
      i++;
    } // end for
    // return our beautiful result ;)
    return (weightedSum / weightSum);
  }

  /**
   * <code>isHorizontal</code> returns true if the LineInfo <code>l</code>
   * represents a horizontal line DONE: 100%
   * 
   * @param l -
   *          LineInfo to analyze
   * @return boolean - true if LineInfo represents a horizontal line, false
   *         otherwise
   */
  private final boolean isHorizontal(final LineInfo l) {

    final Line line = this.sConf.getLine(l.getId());
    return line != null && line.getPoint1().y == line.getPoint2().y;
  }

  /**
   * Tells if an object should be seen.
   * 
   * @param pos
   * @return true if object in viewCone or visibleDistance
   */
  public boolean shouldBeSeen(final StaticObject o) {

    if (o instanceof Ball) {
      return Math.abs(this.me.getAngleRel(o.getPosition(), RELATIVE.TO_HEAD)) < (this.world
          .getViewWidth() - 15.0) / 2 || this.me.getPosition().getDistance(
          o.getPosition()) < this.sConf.VISIBLE_DISTANCE - 0.5;
    }
    else {
      return Math.abs(this.me.getAngleRel(o.getPosition(), RELATIVE.TO_HEAD)) < (this.world
          .getViewWidth()) / 2 || this.me.getPosition().getDistance(
          o.getPosition()) < this.sConf.VISIBLE_DISTANCE;
    }
  }

  /**
   * <code>getBodyDir</code>
   * 
   * @return double
   * @see robocup.component.worldobjects.Player#getBodyDir()
   */
  public final double getBodyDir() {

    if (this.me == null) {
      return 0.0;
    }

    return this.me.getBodyDir();
  }

  /**
   * <code>getHeadDir</code>
   * 
   * @return double
   * @see robocup.component.worldobjects.Player#getHeadDir()
   */
  public final double getHeadDir() {

    return this.me.getHeadDir();
  }

  /**
   * <code>getNeckAngle</code>
   * 
   * @return double
   * @see robocup.component.worldobjects.Player#getNeckAngle()
   */
  public final double getNeckAngle() {

    return this.me.getNeckAngle();
  }

  /**
   * <code>getNumber</code>
   * 
   * @return int
   * @see robocup.component.worldobjects.Player#getNumber()
   */
  public final int getNumber() {

    return this.me.getNumber();
  }

  /**
   * <code>getSpeed</code>
   * 
   * @return The player's velocity. Note that if you perform operations on this
   *         Vektor that change it, you need to clone it first!
   */
  public Vektor getSpeed() {

    return this.me.getSpeed();
  }

  /**
   * <code>getPosition</code>
   * 
   * @return Returns the player's position. Note that if you perform operations
   *         on this Vektor that change it, you need to clone it first!
   */
  public Vektor getPosition() {

    return this.me.getPosition();
  }

  /**
   * @return the view width in degree.
   */
  public final double getViewWidth() {

    return this.viewWidth;
  }

  /**
   * @return Returns the me as clone.
   */
  public Player getPlayerClone() {

    return this.me.cloned();
  }

  public int getPlayerType() {

    return this.playerType;
  }

  public void setPlayerType(final int playerType) {

    this.playerType = playerType;
  }

  /**
   * @return Returns the lastMe.
   */
  public Player getLastMe() {

    return this.lastMe;
  }

  /**
   * @return Returns the predictedMe.
   */
  public Player getPredictedMe() {

    return this.predictedMe;
  }

  /**
   * resets the particle filters after we collided
   */
  public void resetFilterAfterCollisionWithBall() {

    this.mclFilter.reset(this.me.getPosition());
    this.particleFilter.resetPosition(this.me.getPosition());
  }

  /* (non-Javadoc)
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerTypesInfo)
   */
  public void update(final PlayerTypesInfo info) {

    // TODO Auto-generated method stub
    
  }

}
